#include <cstdint>
#include <iostream>
#include <ros/ros.h>
#include <serial/serial.h>
#include <string>
#include <sys/types.h>

#include "serial_to_stm32/gimbal_mimic_data.h"

#define DEBUG_KEY_INPUT

#define PI 3.1415926536
//This is PI

typedef struct {
  uint8_t sof;
  float gimbal_yaw;
  float gimbal_pitch;
  uint8_t shoot_status1;
  uint8_t shoot_status2;
  uint8_t rev;
  uint8_t eof;
} Gimbal_Mimic_Data_t;


void serial_init(serial::Serial *sp);
void data_format(Gimbal_Mimic_Data_t *data_ptr, uint8_t txbuff[], int32_t temp[]);
void GimbalControlCallback(const serial_to_stm32::gimbal_mimic_data::ConstPtr& msg);
float loop_limit(float input);

serial::Serial sp;

Gimbal_Mimic_Data_t Gimbal_Mimic_Data;
serial_to_stm32::gimbal_mimic_data gimbal_feedback_data;

int32_t temp[4];
uint8_t txbuff[1024];
uint8_t rxbuff[1024];

int last_rev;
 
 
int main(int argc, char **argv) {
  ros::init(argc, argv, "mimic_node");
  //创建句柄
  ros::NodeHandle n;
  
  //创建订阅者，订阅名为 /gimbal_control 的topic
  ros::Subscriber gimbal_control_sub = n.subscribe("/gimbal_control", 10, GimbalControlCallback);
  
  //创建发布者，发布名为 /gimbal_feedback 的topic
  ros::Publisher gimbal_feedback_pub = n.advertise<serial_to_stm32::gimbal_mimic_data>("/gimbal_feedback", 10);
  
  //创建一个serial对象
  serial_init(&sp);
  
  try {
    //打开串口
    sp.open();
  } catch (serial::IOException &e) {
    ROS_ERROR_STREAM("Unable to open port.");
    return -1;
  }

  //判断串口是否打开成功
  if (sp.isOpen()) {
    ROS_INFO_STREAM("port is opened.");
  } else {
    return -1;
  }  
    
  //初始化云台发送数据
  Gimbal_Mimic_Data.sof = 0x22;
  Gimbal_Mimic_Data.gimbal_pitch = 0;
  Gimbal_Mimic_Data.gimbal_yaw = 0;
  Gimbal_Mimic_Data.shoot_status1 = 0;
  Gimbal_Mimic_Data.shoot_status2 = 0;
  Gimbal_Mimic_Data.rev = 0;
  Gimbal_Mimic_Data.eof = 0x33;
  

  ros::Rate loop_rate(1000);
  while (ros::ok()) {
  
     ros::spinOnce();
     
    //获取缓冲区内的字节数
    size_t n = sp.available();
    
    if (n>0) {
      n = sp.read(rxbuff, n);
      for (int i = 0; i < n; i++) {
        std::cout << std::hex << (int)rxbuff[i] << " ";
      }
      std::cout << std::endl;
    }
    

    /*
    int i;
    if (n >= 50) {
      // uint8_t buffer[1024];
      //读出数据
      //n = sp.read(rxbuff, n);
      sp.read(rxbuff, n);
        for(i = 0 ; i < n - 12 ; i++ )
        {
          if(rxbuff[i] == 0x55 && rxbuff[i + 12] == 0x66) break; 
        }
      
        //整理云台反馈数据
        if (i < (n - 12) )
        {
          last_rev = gimbal_feedback_data.rev;
          temp[2] =  rxbuff[1 + i] << 24 | rxbuff[2 + i] << 16 | rxbuff[3 + i] << 8 | rxbuff[4 + i];
	        gimbal_feedback_data.gimbal_yaw = temp[2] / 100000.0f;
	        temp[3] =  rxbuff[5 + i] << 24 | rxbuff[6 + i] << 16 | rxbuff[7 + i] << 8 | rxbuff[8 + i];
	        gimbal_feedback_data.gimbal_pitch = temp[3] / 100000.0f;
      
          gimbal_feedback_data.shoot_status1 = rxbuff[9 + i];
          gimbal_feedback_data.shoot_status2 = rxbuff[10 + i];
          gimbal_feedback_data.rev = rxbuff[11 + i];
        
          gimbal_feedback_pub.publish(gimbal_feedback_data);
          //ROS_INFO("receive gimbal_feedback");
        }
        else
        {
          //ROS_ERROR("receive wrong data");
        }
        
        for(int j = 0 ; j < 13 ; j++)
        {
          std::cout << "[" << j <<"]:" << std::hex << (int)rxbuff[i + j] << " ";
        }
        std::cout << std::endl;
        /*
        for (int i = 0; i < n; i++) {
           //std::cout  << i;
          // 16进制的方式打印到屏幕
          //std::cout << std::hex << (rxbuff[i] & 0xff) << " ";
           //std::cout << std::hex << (int)rxbuff[i] << "\t";
        }
       //std::cout << "gimbal_feedback:\nyaw: " << gimbal_feedback_data.gimbal_yaw << "\npitch: " << gimbal_feedback_data.gimbal_pitch
       //           << "\nstatus1:" << gimbal_feedback_data.shoot_status1 << "\nstatus2" << gimbal_feedback_data.shoot_status2 << "\nrev"
       //           << gimbal_feedback_data.rev;
        std::cout << std::endl;
      
      }
      else
      {
        //ROS_INFO("not enough data");
        //std::cout << "n=" << n << std::endl;
      }
      
      if(gimbal_feedback_data.rev == 0)
      {
         Gimbal_Mimic_Data.gimbal_pitch = gimbal_feedback_data.gimbal_pitch;
         Gimbal_Mimic_Data.gimbal_yaw = gimbal_feedback_data.gimbal_yaw;
      }
        */
    loop_rate.sleep();
  }

  //关闭串口
  sp.close();

  return 0;
}


void serial_init(serial::Serial *sp)
{
  //创建timeout
  serial::Timeout to = serial::Timeout::simpleTimeout(100);
  //设置要打开的串口名称
  sp->setPort("/dev/ttyACM0");
  // sp->setPort("/dev/ttyACM1");

  //设置串口通信的波特率
  sp->setBaudrate(100000);
  //串口设置timeout
  sp->setTimeout(to);
}

void data_format(Gimbal_Mimic_Data_t *data_ptr, uint8_t txbuff[], int32_t temp[])
{
  int i;
  txbuff[0] = data_ptr->sof;
  
  data_ptr->gimbal_yaw = loop_limit(data_ptr->gimbal_yaw);
       
   data_ptr->gimbal_pitch = 
   (data_ptr->gimbal_pitch >= 0.49) ? (0.49) : ((data_ptr->gimbal_pitch <= -0.67) ? -0.67 : data_ptr->gimbal_pitch);
  
  temp[0] = data_ptr->gimbal_yaw * 1e+5;
  temp[1] = data_ptr->gimbal_pitch * 1e+5;

  for (i = 0; i < 4; i++) {
    txbuff[1 + i] = temp[0] >> (24 - 8 * i);
  }
  for (i = 0; i < 4; i++) {
    txbuff[5 + i] = temp[1] >> (24 - 8 * i);
  }
  
  txbuff[9] = data_ptr->shoot_status1;
  txbuff[10] = data_ptr->shoot_status2;
  txbuff[12] = data_ptr->eof;
}

void GimbalControlCallback(const serial_to_stm32::gimbal_mimic_data::ConstPtr& msg)
{
  if(last_rev == 0 && gimbal_feedback_data.rev == 1)
  {
     //ROS_INFO("swith into auto mode");
     Gimbal_Mimic_Data.gimbal_pitch = gimbal_feedback_data.gimbal_pitch;
     Gimbal_Mimic_Data.gimbal_yaw = gimbal_feedback_data.gimbal_yaw;
  }
  else if(last_rev == 1 && gimbal_feedback_data.rev == 0)
  {
    //ROS_INFO("swith out auto mode");
  }
  else
  {
    Gimbal_Mimic_Data.gimbal_yaw = msg->gimbal_yaw;
    Gimbal_Mimic_Data.gimbal_pitch = msg->gimbal_pitch;
  }
   
  Gimbal_Mimic_Data.shoot_status1 = msg->shoot_status1;
  Gimbal_Mimic_Data.shoot_status2 = msg->shoot_status2;
  Gimbal_Mimic_Data.rev = msg->rev;
  
  //ROS_INFO("receive gimbal_control:\nyaw: %f,  pitch: %f,  status1: %d,  status2: %d  rev: %d\r\n",
    //        msg->gimbal_yaw, msg->gimbal_pitch, msg->shoot_status1, msg->shoot_status2, msg->rev);
            
  //整理数据
  data_format(&Gimbal_Mimic_Data, txbuff, temp);
  //发送数据
  sp.write(txbuff, 13);
  
  
}

float loop_limit(float input)
{
  while( input <= -PI )
  {
    input += 2 * PI;
  }
  
  while(input >= PI)
  {
    input -= 2 * PI;
  }
  
  return input;
}


